﻿//#include <opencv2/core/core.hpp>

#include <iostream>
#include <fstream>
#include <string>
#include <iterator>

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/search/organized.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/conditional_euclidean_clustering.h>
#include <pcl/features/don.h>
#include <pcl/visualization/cloud_viewer.h>

int user_data;


//这个是读CSV文件然后将读出来的数据返回为一个名为datalist的vector容器列表中
//返回的这个vector中的每一行都是一个vector，然后每行的vector的每一行是一个string

std::vector<std::vector<std::string>>
ReadDataFromCSV(std::string filename)
{
	std::ifstream file(filename);
	std::vector<std::vector<std::string> > dataList;

	std::string line = "";
	while (getline(file, line))
	{
		std::vector<std::string> vec;
		boost::algorithm::split(vec, line, boost::is_any_of(","));
		dataList.push_back(vec);
	}
	return dataList;
}



//输入1：一个vector-vector-string，输入2：类型为XYZI的点云Ptr结构
//输出是void
//函数功能：将输入的rawData中的点云数据赋值给Ptr类型的cloud中，
//		然后保存为final_project_point_cloud.pcd		
void
pcdWriter(std::vector<std::vector<std::string>> rawData, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
	//pcl::PointCloud<pcl::PointXYZI> cloud1;

	// Fill in the cloud data
	cloud->width = (uint32_t)rawData.size();
	cloud->height = 1;
	cloud->is_dense = false;
	cloud->points.resize(cloud->width * cloud->height);

	for (size_t i = 0; i < cloud->points.size(); ++i)
	{
		//std::string::size_type sz;     // alias of size_t
		cloud->points[i].x = std::stof(rawData[i][0]);
		cloud->points[i].y = std::stof(rawData[i][1]);
		cloud->points[i].z = std::stof(rawData[i][2]);
		cloud->points[i].intensity = std::stof(rawData[i][3]);
	}

	pcl::io::savePCDFileASCII("final_project_point_cloud.pcd", *cloud);
	std::cerr << "Saved " << cloud->points.size() << " data points to final_project_point_cloud.pcd file." << std::endl;

	//for (size_t i = 0; i < cloud->points.size(); ++i)
	//	std::cerr << "    " << cloud->points[i].x << " " << cloud->points[i].y << " " << cloud->points[i].z << std::endl;
}


//函数DoN  Writer
//输入：一个PointNormal类型 Ptr格式的点云格式，一个文件路径fileName
//PointNormal点云类型： X Y Z curvature（曲率）
//输出：void
//函数功能：
//	将输入的PointNormal类型 Ptr格式的点云中的curvature数据提取出来写到filename这个文件中

void
DoNWriter(pcl::PointCloud<pcl::PointNormal>::Ptr &cloud, std::string fileName) {
	std::vector<float> mag;
	std::ofstream out(fileName);
	for (size_t i = 0; i < cloud->points.size(); ++i)
	{
		//std::string::size_type sz;     // alias of size_t
		mag.push_back(cloud->points[i].curvature);
	}

	//创建一个out_iterator的迭代器，用来向out这个输出流添加元素,并且以回车为间隔符
	std::ostream_iterator<float> out_iterator(out, "\n");
	//将名为mag的vector中的所有元素添加到out_iterator这个迭代器中
	std::copy(mag.begin(), mag.end(), out_iterator);
	out.close();

	//for (size_t i = 0; i < cloud->points.size(); ++i)
	//	std::cerr << "    " << cloud->points[i].x << " " << cloud->points[i].y << " " << cloud->points[i].z << std::endl;
}

//cloudGrandRemover函数
//输入：只有一个类型为PointNormal的Ptr点云
//输出：void
//函数功能：对originCloud点云进行平面分割，获得分割后的平面点云和平面参数，并将originCloud更改为剔除平面点云后的点云

void
cloudGrandRemover(pcl::PointCloud<pcl::PointNormal>::Ptr &originCloud) {
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::copyPointCloud<pcl::PointNormal, pcl::PointXYZ>(*originCloud, *cloud);

	std::cout << "Remove grand plane..." << std::endl;

	//创建分割时所需要的模型系数对象，coefficients及存储内点的点索引集合对象inliers
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	//创建一个名为inliers的点云索引
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
	// Create the segmentation object创建分割对象seg
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	// Optional可选择配置，设置模型系数需要优化
	seg.setOptimizeCoefficients(true);
	// Mandatory
	seg.setModelType(pcl::SACMODEL_PLANE);//设置模型类型
	seg.setMethodType(pcl::SAC_RANSAC);//设置采样一致性类型，这里用的RANSAC
	seg.setDistanceThreshold(0.1);//设定距离阀值，距离阀值决定了点被认为是局内点是必须满足的条件

	seg.setInputCloud(cloud);//设置分割对象seg的输入点云为cloud
	//储存分割后的点云序列到inliers，储存分割好的平面系数到coefficients
	//开始分割
	seg.segment(*inliers, *coefficients);

	std::cerr << "Model coefficients: " << coefficients->values[0] << " "
		<< coefficients->values[1] << " "
		<< coefficients->values[2] << " "
		<< coefficients->values[3] << std::endl;

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_seg(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());

	// Extract the planar inliers from the input cloud
	//根据索引inliers，利用提取器extract将cloud里对应索引的点提取出来
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(cloud);
	extract.setIndices(inliers);
	extract.setNegative(false);

	// Get the points associated with the planar surface
	//提取出的点云储存在cloud_seg中
	extract.filter(*cloud_seg);
	std::cout << "PointCloud representing the planar component: " << cloud_seg->points.size() << " data points." << std::endl;

	// Remove the planar inliers, extract the rest移除平面索引，输出剩下的点云

	//extract.setNegative这里如果是false，就是提取索引对应的点云；如果是false，就是提取非索引点云
	//
	extract.setNegative(true);
	//提取非索引点云储存在cloud_filtered中
	extract.filter(*cloud_filtered);
	//将cloud_filtered中的数据覆盖掉originCloud中的数据，并且由于PointNormal有XYZ 和curvature，所以输出的结果是否带有curvature字段呢？？
	pcl::copyPointCloud<pcl::PointXYZ, pcl::PointNormal>(*cloud_filtered, *originCloud);
}


//函数DiffNormalSegmentation
//输入：结构为PointXYZI的Prt点云cloud。结构为PointXYZRGBA的Prt点云segCloud
//输出：void
//函数功能：对输入的点云cloud 创建KD树，计算每个点的法线，然后将法线输出为MagDoN.txt
			//对输入的点云cloud计算Don分割 输出分割后点云don.pcd
			//然后对分割的点云进行平面提取，得到平面点云
			//再构建分类器，进行欧氏距离聚类
			//最后对聚类的点云进行赋予不同的颜色，并输出segCould.pcd
//最后输出的是第二个点云参数segCloud
			

void
DiffNormalSegmentation(pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &segCloud) {
	double scale1 = 0.06;//The smallest scale to use in the DoN filter.	应该是计算法向量时的最小距离
	double scale2 = 0.6;//The largest scale to use in the DoN filter.	应该是计算法向量时的最大距离
	double threshold = 0.5;//The minimum DoN magnitude to threshold by  最小DON量的阈值设置
	double segradius = 0.6;//segment scene into clusters with given distance tolerance using euclidean clustering设定给定距离，为了使用欧几里得聚类法将场景分割

	// Create a search tree, use KDTreee for non-organized data.
	//创建KD树
	std::cout << "creating search tree using KDTree" << std::endl;
	pcl::search::Search<pcl::PointXYZI>::Ptr tree;
	if (cloud->isOrganized())
	{
		tree.reset(new pcl::search::OrganizedNeighbor<pcl::PointXYZI>());
	}
	else
	{
		tree.reset(new pcl::search::KdTree<pcl::PointXYZI>(false));
	}

	// Set the input pointcloud for the search tree
	tree->setInputCloud(cloud);

	if (scale1 >= scale2)
	{
		cerr << "Error: Large scale must be > small scale!" << endl;
		exit(EXIT_FAILURE);
	}

	//********************************************DoN***********************************************************************
	// Compute normals using both small and large scales at each point
	//使用最小和最大距离分别计算法线
	pcl::NormalEstimationOMP<pcl::PointXYZI, pcl::PointNormal> ne;
	ne.setInputCloud(cloud);
	ne.setSearchMethod(tree);
	ne.setNumberOfThreads(7);

	/**
	* NOTE: setting viewpoint is very important, so that we can ensure
	* normals are all pointed in the same direction!
	*/
	//ne.setViewPoint(std::numeric_limits<float>::max(), std::numeric_limits<float>::max(), std::numeric_limits<float>::max());
	ne.setViewPoint(-50, -50, -50);

	// calculate normals with the small scale
	cout << "Calculating normals for scale..." << scale1 << endl;
	pcl::PointCloud<pcl::PointNormal>::Ptr normals_small_scale(new pcl::PointCloud<pcl::PointNormal>);

	ne.setRadiusSearch(scale1);
	ne.compute(*normals_small_scale);

	// calculate normals with the large scale
	cout << "Calculating normals for scale..." << scale2 << endl;
	pcl::PointCloud<pcl::PointNormal>::Ptr normals_large_scale(new pcl::PointCloud<pcl::PointNormal>);

	ne.setRadiusSearch(scale2);
	ne.compute(*normals_large_scale);

	// Create output cloud for DoN results
	pcl::PointCloud<pcl::PointNormal>::Ptr doncloud(new pcl::PointCloud<pcl::PointNormal>);
	pcl::copyPointCloud<pcl::PointXYZI, pcl::PointNormal>(*cloud, *doncloud);
	cout << "Calculating DoN... " << endl;
	// Create DoN operator
	pcl::DifferenceOfNormalsEstimation<pcl::PointXYZI, pcl::PointNormal, pcl::PointNormal> don;
	don.setInputCloud(cloud);
	don.setNormalScaleLarge(normals_large_scale);
	don.setNormalScaleSmall(normals_small_scale);

	if (!don.initCompute())
	{
		std::cerr << "Error: Could not initialize DoN feature operator" << std::endl;
		exit(EXIT_FAILURE);
	}

	// Compute DoN
	don.computeFeature(*doncloud);

	// Save DoN features
	pcl::PCDWriter writer;
	writer.write<pcl::PointNormal>("don.pcd", *doncloud, false);

	//调用函数DoNWriter
	//将输入的PointNormal类型 Ptr格式的点云中的curvature数据提取出来写到filename这个文件中
	DoNWriter(doncloud, "MagDoN.txt");
	//********************************************End of DoN***********************************************************************

	//********************************************Filtering************************************************************************
	// Filter by magnitude按照大小过滤
	cout << "Filtering out DoN mag <= " << threshold << "..." << endl;

	// Build the condition for filtering
	pcl::ConditionOr<pcl::PointNormal>::Ptr range_cond(
		new pcl::ConditionOr<pcl::PointNormal>()
	);
	range_cond->addComparison(pcl::FieldComparison<pcl::PointNormal>::ConstPtr(
		new pcl::FieldComparison<pcl::PointNormal>("curvature", pcl::ComparisonOps::GT, threshold))
	);
	// Build the filter
	pcl::ConditionalRemoval<pcl::PointNormal> condrem;
	condrem.setCondition(range_cond);
	//pcl::PointIndices::Ptr donOutliers(new pcl::PointIndices);
	condrem.setInputCloud(doncloud);

	pcl::PointCloud<pcl::PointNormal>::Ptr doncloud_filtered(new pcl::PointCloud<pcl::PointNormal>);

	// Apply filter
	condrem.filter(*doncloud_filtered);
	//donOutliers = condrem.getRemovedIndices();

	doncloud = doncloud_filtered;

	//*******************************************************************************************************************************
	//这里调用了cloudGrandRemover函数
	//对originCloud点云进行平面分割，获得分割后的平面点云和平面参数，并将originCloud更改为剔除平面点云后的点云
	cloudGrandRemover(doncloud);
	std::cout << "Plane removed Pointcloud: " << doncloud->points.size() << " data points." << std::endl;
	pcl::copyPointCloud<pcl::PointNormal, pcl::PointXYZRGBA>(*doncloud, *segCloud);
	//*******************************************************************************************************************************

	// Save filtered output
	std::cout << "Filtered Pointcloud: " << doncloud->points.size() << " data points." << std::endl;

	writer.write<pcl::PointNormal>("don_filtered.pcd", *doncloud, false);
	//********************************************End of Filtering*******************************************************************

	//********************************************Clustering*************************************************************************
	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr doncloud_intermediate(new pcl::PointCloud<pcl::PointXYZRGBA>);
	pcl::copyPointCloud<pcl::PointNormal, pcl::PointXYZRGBA>(*doncloud, *doncloud_intermediate);

	cout << "Clustering using EuclideanClusterExtraction with tolerance <= " << segradius << "..." << endl;

	pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr segtree(new pcl::search::KdTree<pcl::PointXYZRGBA>);
	segtree->setInputCloud(doncloud_intermediate);

	std::vector<pcl::PointIndices> cluster_indices;
	//定义一个新的欧式距离聚类器ec
	pcl::EuclideanClusterExtraction<pcl::PointXYZRGBA> ec;

	ec.setClusterTolerance(segradius);
	ec.setMinClusterSize(100);
	ec.setMaxClusterSize(40000);
	ec.setSearchMethod(segtree);

	ec.setInputCloud(doncloud_intermediate);//doncloud
	ec.extract(cluster_indices);
	//输出了一个点云索引↑↑↑↑
	std::cout << "Segmenting point cloud and add colors..." << std::endl;
	//下面应该是对聚类出来的点云进行一个着色处理，变成红色
	int j = 0;
	uint8_t red = 255;
	uint8_t green = 0;
	uint8_t blue = 0;
	for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it, j++)
	{
		pcl::PointCloud<pcl::PointNormal>::Ptr cloud_cluster_don(new pcl::PointCloud<pcl::PointNormal>);
		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
		{
			cloud_cluster_don->points.push_back(doncloud->points[*pit]);
			segCloud->at(*pit).r = red;
			segCloud->at(*pit).g = green;
			segCloud->at(*pit).b = blue;
			segCloud->at(*pit).a = 255;
		}
		//颜色改为一个随机颜色
		red = rand() % 256;
		green = rand() % 256;
		blue = rand() % 256;

		cloud_cluster_don->width = int(cloud_cluster_don->points.size());
		cloud_cluster_don->height = 1;
		cloud_cluster_don->is_dense = true;

		//Save cluster
		//因为聚类器会聚类很多个类，对每一个类都输出一个don_cluster_j.pcd;
		cout << "PointCloud representing the Cluster: " << cloud_cluster_don->points.size() << " data points." << std::endl;
		std::stringstream ss;
		ss << "don_cluster_" << j << ".pcd";
		writer.write<pcl::PointNormal>(ss.str(), *cloud_cluster_don, false);
		ss << "Mag_don_cluster_" << j << ".txt";

		//调用函数DoNWriter
		//将输入的PointNormal类型 Ptr格式的点云中的curvature数据提取出来写到filename这个文件中
		DoNWriter(cloud_cluster_don, ss.str());
	}
	writer.write<pcl::PointXYZRGBA>("segCould.pcd", *segCloud, false);
}


//这个函数就是对viewer进行的一些相关的参数设置
//就是读取trajectory_camera_coord.csv然后获得球体数据，并在viewer中添加球体（addsphere）
void
viewerOneOff(pcl::visualization::PCLVisualizer& viewer)
{
	viewer.setBackgroundColor(0.1, 1.0, 0.7);
	std::vector<std::vector<std::string>> trajectory = ReadDataFromCSV("trajectory_camera_coord.csv");
	for (int i = 0; i < trajectory.size(); ++i) {
		pcl::PointXYZ o;
		o.x = std::stof(trajectory[i][0]);
		o.y = std::stof(trajectory[i][1]);
		o.z = std::stof(trajectory[i][2]);
		viewer.addSphere(o, 10, "sphere", 0);
	}
	std::cout << "this only runs once" << std::endl;

}

//这就是一个显示函数吧，显示viewer
void
viewerPsycho(pcl::visualization::PCLVisualizer& viewer)
{
	static unsigned count = 0;
	std::stringstream ss;
	ss << "Once per viewer loop: " << count++;
	viewer.removeShape("text", 0);
	viewer.addText(ss.str(), 200, 300, "text", 0);

	//FIXME: possible race condition here:
	user_data++;
}

int
main()
{
	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr segCloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
	//std::cout << "Reading data from csv file..." << std::endl;
	//std::vector<std::vector<std::string> > raw_data = ReadDataFromCSV("point_cloud_camera_coord.csv");
	//std::cout << "Writing data into pcd file..." << std::endl;
	//pcdWriter(raw_data, cloud);
	//raw_data.clear();

	std::string fileName = "final_project_point_cloud.pcd";
	std::cout << "Loading point cloud from file: " << fileName << std::endl;
	pcl::PCLPointCloud2 cloud_blob;
	pcl::io::loadPCDFile(fileName, cloud_blob);
	pcl::fromPCLPointCloud2(cloud_blob, *cloud);

	DiffNormalSegmentation(cloud, segCloud);
	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr finalCloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
	pcl::copyPointCloud<pcl::PointXYZI, pcl::PointXYZRGBA>(*cloud, *finalCloud);
	*finalCloud += *segCloud;//想当于 finalCloud=finalCloud+segCloud
	pcl::PCDWriter writer;
	writer.write<pcl::PointXYZRGBA>("final.pcd", *finalCloud, false);

	pcl::visualization::CloudViewer viewer("Cloud Viewer");
	//blocks until the cloud is actually rendered
	viewer.showCloud(finalCloud);

	//use the following functions to get access to the underlying more advanced/powerful
	//PCLVisualizer

	//This will only get called once

	//viewer.runOnVisualizationThreadOnce(viewerOneOff);

	//This will get called once per visualization iteration
	viewer.runOnVisualizationThread(viewerPsycho);
	while (!viewer.wasStopped())
	{
		//you can also do cool processing here
		//FIXME: Note that this is running in a separate thread from viewerPsycho
		//and you should guard against race conditions yourself...
		user_data++;
	}

	cout << "hello world" << endl;
	return 0;
}
